(* Content-type: application/vnd.wolfram.mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 10.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       158,          7]
NotebookDataLength[     12289,        260]
NotebookOptionsPosition[     11764,        240]
NotebookOutlinePosition[     12173,        258]
CellTagsIndexPosition[     12130,        255]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell[TextData[StyleBox["\[OpenCurlyQuote]Reeds-Shepp  Curve\[CloseCurlyQuote] \
     VS        \[OpenCurlyQuote]Dubins  Curve\[CloseCurlyQuote]",
 FontFamily->"Georgia",
 FontColor->RGBColor[0., 0., 0.]]], "Section",
 CellChangeTimes->{{3.624778911347768*^9, 3.62477896523685*^9}, {
   3.624879304189974*^9, 3.6248793291200085`*^9}, 3.6288407625695314`*^9, {
   3.771618316586*^9, 3.7716183224110003`*^9}, {3.7732206512574964`*^9, 
   3.7732206769138994`*^9}, {3.7732213709882565`*^9, 3.773221400257133*^9}},
 FontSize->24,ExpressionUUID->"def1c7f9-814a-4be2-ac2c-439a82adb519"],

Cell[BoxData[{
 RowBox[{
  RowBox[{"Off", "[", 
   RowBox[{"General", "::", 
    StyleBox["shdw", "MessageName"]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"<<", "ReedsShepp.m"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"<<", "Dubins.m"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"Manipulate", "[", "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"Dubins`Rmin", "=", "R"}], ";", "\[IndentingNewLine]", 
    RowBox[{"ReedsShepp`Rmin", "=", "R"}], ";", "\[IndentingNewLine]", 
    RowBox[{"start", "=", 
     RowBox[{"Flatten", "@", 
      RowBox[{"{", 
       RowBox[{"StartPos", ",", "\[Theta]s"}], "}"}]}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"target", "=", 
     RowBox[{"Flatten", "@", 
      RowBox[{"{", 
       RowBox[{"TargetPos", ",", "\[Theta]t"}], "}"}]}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"stepsize", "=", "0.01"}], ";", "\[IndentingNewLine]", 
    RowBox[{"DubinsCurve", "=", 
     RowBox[{"DubinsPathSampleMany", "[", 
      RowBox[{"start", ",", "target", ",", "stepsize"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"pathx", "=", 
     RowBox[{"pathy", "=", 
      RowBox[{"path\[Theta]", "=", 
       RowBox[{"{", "}"}]}]}]}], ";", " ", "\[IndentingNewLine]", 
    RowBox[{"ReedShepp", "[", 
     RowBox[{"start", ",", "target"}], "]"}], ";", "\[IndentingNewLine]", 
    RowBox[{"constRS", "[", 
     RowBox[{"num", ",", 
      RowBox[{"{", 
       RowBox[{"RSl1", ",", "RSl2", ",", "RSl3"}], "}"}], ",", "start", ",", 
      "stepsize"}], "]"}], ";", "\[IndentingNewLine]", 
    RowBox[{"ReedsSheppCurve", "=", 
     RowBox[{"Transpose", "[", 
      RowBox[{"{", 
       RowBox[{"pathx", ",", "pathy", ",", "path\[Theta]"}], "}"}], "]"}]}], 
    ";", "\[IndentingNewLine]", 
    RowBox[{"Graphics", "[", 
     RowBox[{
      RowBox[{"{", "\[IndentingNewLine]", 
       RowBox[{"Black", ",", 
        RowBox[{"Thickness", "[", "0.007", "]"}], ",", 
        RowBox[{"Line", "[", 
         RowBox[{"ReedsSheppCurve", "\[LeftDoubleBracket]", 
          RowBox[{";;", ",", 
           RowBox[{"1", ";;", "2"}]}], "\[RightDoubleBracket]"}], "]"}], ",", 
        "\[IndentingNewLine]", "Red", ",", 
        RowBox[{"Thickness", "[", "0.003", "]"}], ",", 
        RowBox[{"Line", "[", 
         RowBox[{"DubinsCurve", "\[LeftDoubleBracket]", 
          RowBox[{";;", ",", 
           RowBox[{"1", ";;", "2"}]}], "\[RightDoubleBracket]"}], "]"}], ",", 
        "\[IndentingNewLine]", "Blue", ",", 
        RowBox[{"Thickness", "[", "0.003", "]"}], ",", 
        RowBox[{"Arrowheads", "[", "0.03", "]"}], ",", 
        RowBox[{"Arrow", "[", 
         RowBox[{"{", 
          RowBox[{"StartPos", ",", 
           RowBox[{"StartPos", "+", 
            RowBox[{"0.7", "*", 
             RowBox[{"{", 
              RowBox[{
               RowBox[{"Cos", "[", "\[Theta]s", "]"}], ",", 
               RowBox[{"Sin", "[", "\[Theta]s", "]"}]}], "}"}]}]}]}], "}"}], 
         "]"}], ",", "\[IndentingNewLine]", "Green", ",", 
        RowBox[{"Arrow", "[", 
         RowBox[{"{", 
          RowBox[{"TargetPos", ",", 
           RowBox[{"TargetPos", "+", 
            RowBox[{"0.7", "*", 
             RowBox[{"{", 
              RowBox[{
               RowBox[{"Cos", "[", "\[Theta]t", "]"}], ",", 
               RowBox[{"Sin", "[", "\[Theta]t", "]"}]}], "}"}]}]}]}], "}"}], 
         "]"}]}], "}"}], ",", 
      RowBox[{"PlotRange", "\[Rule]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{
            RowBox[{"-", "1.5"}], ",", "1.5"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"-", "1"}], ",", "1"}], "}"}]}], "}"}], "*", "3"}]}], ",", 
      RowBox[{"Axes", "\[Rule]", "False"}]}], "]"}]}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"StartPos", ",", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"-", "1.9"}], ",", 
         RowBox[{"-", "0.7"}]}], "}"}]}], "}"}], ",", "Locator"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"TargetPos", ",", 
       RowBox[{"{", 
        RowBox[{"0.6", ",", "0.8"}], "}"}]}], "}"}], ",", "Locator"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"\[Theta]s", ",", 
       RowBox[{"-", "0.5"}]}], "}"}], ",", 
     RowBox[{"-", "Pi"}], ",", "Pi", ",", 
     RowBox[{"ImageSize", "\[Rule]", "Large"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"\[Theta]t", ",", "0.14"}], "}"}], ",", 
     RowBox[{"-", "Pi"}], ",", "Pi", ",", 
     RowBox[{"ImageSize", "\[Rule]", "Large"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"R", ",", "0.6"}], "}"}], ",", "0.00001", ",", "1", ",", 
     RowBox[{"ImageSize", "\[Rule]", "Large"}]}], "}"}], ",", 
   RowBox[{"TrackedSymbols", "\[RuleDelayed]", "True"}]}], "]"}]}], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10001.},
 CellChangeTimes->{{3.641880174570367*^9, 3.6418801941283994`*^9}, {
   3.641880235868471*^9, 3.6418802687245417`*^9}, {3.641880299500594*^9, 
   3.6418803559346943`*^9}, {3.64188038652874*^9, 3.6418804353808217`*^9}, {
   3.641880475762952*^9, 3.6418804996689887`*^9}, {3.641880534695045*^9, 
   3.641880586551128*^9}, {3.641880634847205*^9, 3.6418806354092073`*^9}, {
   3.641880735669385*^9, 3.6418807637534523`*^9}, {3.641880799671509*^9, 
   3.641880846065589*^9}, {3.641880879283646*^9, 3.6418808845836535`*^9}, {
   3.641881572703932*^9, 3.641881644352078*^9}, {3.641882446556145*^9, 
   3.6418824515161524`*^9}, 3.6418825262822914`*^9, {3.641882663956523*^9, 
   3.641882667276528*^9}, {3.641882765360711*^9, 3.641882774652725*^9}, {
   3.641882890754907*^9, 3.6418829072509403`*^9}, {3.6418841598548126`*^9, 
   3.641884162881218*^9}, {3.6418842506625724`*^9, 3.6418843485215445`*^9}, {
   3.641884447864591*^9, 3.641884456796102*^9}, {3.6418848867132115`*^9, 
   3.641884895433627*^9}, {3.641885582926822*^9, 3.6418855918473325`*^9}, {
   3.6418856492825327`*^9, 3.64188567442457*^9}, {3.641886112230339*^9, 
   3.6418861217078815`*^9}, {3.6418862129324975`*^9, 3.64188636233396*^9}, {
   3.641886405140435*^9, 3.641886408510041*^9}, {3.642052248399412*^9, 
   3.6420522529234204`*^9}, 3.6435804813558965`*^9, {3.643580610609289*^9, 
   3.643580790419574*^9}, {3.6435808415094957`*^9, 3.643580882939866*^9}, {
   3.643581254376111*^9, 3.643581274177243*^9}, {3.6435813264142313`*^9, 
   3.6435813485254955`*^9}, {3.6435814478161745`*^9, 
   3.6435814630130444`*^9}, {3.643581504441414*^9, 3.6435815360622225`*^9}, {
   3.6435816096014285`*^9, 3.643581610415475*^9}, {3.6435817658283644`*^9, 
   3.643581805073609*^9}, 3.6435824751449347`*^9, 3.6435871192885647`*^9, {
   3.6435901360311127`*^9, 3.6435901373751893`*^9}, {3.643590172338189*^9, 
   3.643590177544487*^9}, {3.6435902089752846`*^9, 3.64359022987648*^9}, 
   3.6435902948171945`*^9, {3.643769558739024*^9, 3.6437695665084686`*^9}, {
   3.643769609107905*^9, 3.643769701124168*^9}, {3.6437842623020067`*^9, 
   3.643784279515991*^9}, {3.643784314881014*^9, 3.6437843271547165`*^9}, {
   3.6437847728772097`*^9, 3.6437847918382945`*^9}, {3.643785149734765*^9, 
   3.643785151835885*^9}, 3.643785197923521*^9, {3.643785231933467*^9, 
   3.643785245922267*^9}, {3.643791786530907*^9, 3.643791803035736*^9}, {
   3.6437945016373653`*^9, 3.643794527954612*^9}, 3.643884244300424*^9, {
   3.6439266146062593`*^9, 3.643926626098917*^9}, {3.6439266720755467`*^9, 
   3.6439266798539915`*^9}, {3.6439267118538218`*^9, 3.643926759815565*^9}, 
   3.6439329312135487`*^9, 3.6440529837665386`*^9, {3.644053201805997*^9, 
   3.644053207094406*^9}, 3.644053511684268*^9, {3.644481596260764*^9, 
   3.64448159969096*^9}, {3.6445337299797955`*^9, 3.644533759526247*^9}, {
   3.644538098850007*^9, 3.644538112656031*^9}, {3.644538258251087*^9, 
   3.6445382688279057`*^9}, {3.64453982181709*^9, 3.6445398283691015`*^9}, {
   3.6445684078029146`*^9, 3.6445684091869187`*^9}, {3.6445685239442425`*^9, 
   3.644568524174243*^9}, {3.646350873571142*^9, 3.646350909794406*^9}, {
   3.6463588000746236`*^9, 3.6463588090166435`*^9}, {3.6472971224314227`*^9, 
   3.6472971239325085`*^9}, 3.6641006250777082`*^9, {3.6641006562864933`*^9, 
   3.6641006820439663`*^9}, {3.771604125965728*^9, 3.7716041447715864`*^9}, {
   3.7716041905085077`*^9, 3.7716042450643835`*^9}, {3.77160428019486*^9, 
   3.771604284460659*^9}, {3.7716043163450193`*^9, 3.7716045364606485`*^9}, {
   3.7716045739577208`*^9, 3.7716046014331017`*^9}, {3.7716046365709305`*^9, 
   3.7716047022088223`*^9}, {3.771604740258052*^9, 3.771604782848056*^9}, {
   3.7716048222691555`*^9, 3.7716049887532687`*^9}, {3.7716050259786224`*^9, 
   3.771605026891183*^9}, {3.771605112001359*^9, 3.7716052303773108`*^9}, {
   3.77160527488006*^9, 3.7716054269329596`*^9}, {3.771605459641882*^9, 
   3.771605460861644*^9}, {3.771605554777628*^9, 3.7716055856001186`*^9}, 
   3.7716056369801054`*^9, {3.7716058389055357`*^9, 3.77160600012087*^9}, {
   3.7716060914023347`*^9, 3.7716060964089494`*^9}, 3.7716066180899158`*^9, {
   3.771606855300461*^9, 3.771606905781067*^9}, {3.771618429969*^9, 
   3.77161844684*^9}, {3.771618726392*^9, 3.771618757264*^9}, {
   3.771618801104*^9, 3.771618832744*^9}, {3.7716188856879997`*^9, 
   3.77161899224*^9}, {3.7716190275439997`*^9, 3.7716190437679996`*^9}, {
   3.7716191163199997`*^9, 3.771619212968*^9}, {3.77161932284*^9, 
   3.771619369304*^9}, {3.7716193996800003`*^9, 3.77161941824*^9}, {
   3.771619474072*^9, 3.771619474208*^9}, {3.771619520424*^9, 
   3.771619584616*^9}, 3.771619631409*^9, {3.7716196644969997`*^9, 
   3.771619698168*^9}, {3.77161974524*^9, 3.7716197668719997`*^9}, {
   3.7716198006800003`*^9, 3.7716198053120003`*^9}, {3.771619853545*^9, 
   3.771619853976*^9}, {3.771619920817*^9, 3.7716199491619997`*^9}, {
   3.771620070472*^9, 3.7716200909119997`*^9}, {3.771620140198*^9, 
   3.771620162969*^9}, {3.7716202281610003`*^9, 3.77162033156*^9}, 
   3.771620495784*^9, {3.771621819457*^9, 3.771621856585*^9}, 
   3.7716218870959997`*^9, {3.771628666801*^9, 3.7716286740559998`*^9}, {
   3.77322006076972*^9, 3.7732200626117644`*^9}, {3.77322024943548*^9, 
   3.7732202747941933`*^9}, {3.7732206900469155`*^9, 3.7732207266198225`*^9}, 
   3.7732207750064335`*^9, {3.773220834347849*^9, 3.7732208713195953`*^9}, {
   3.7732209490699973`*^9, 3.773220969827908*^9}, {3.7732213247497396`*^9, 
   3.773221343780054*^9}},ExpressionUUID->"185b4002-7d10-4b55-b911-\
56da9ecbbc48"]
},
CellGrouping->Manual,
WindowSize->{1536, 781},
WindowMargins->{{-8, Automatic}, {Automatic, -8}},
Magnification:>1.2 Inherited,
FrontEndVersion->"11.1 for Microsoft Windows (64-bit) (2017\:5e743\:670813\
\:65e5)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[558, 20, 578, 8, 72, "Section", "ExpressionUUID" -> \
"def1c7f9-814a-4be2-ac2c-439a82adb519"],
Cell[1139, 30, 10621, 208, 534, "Input", "ExpressionUUID" -> \
"185b4002-7d10-4b55-b911-56da9ecbbc48",
 CellGroupingRules->{GroupTogetherGrouping, 10001.}]
}
]
*)

